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ABSTRACT 


This  research  involves  the  development  of  an  adaptive  control  law  for  a  space  based 


two-linxv  robotic  manipulator.  Non-adaptive  controllers  are  first  obtained  utilizing  feedback 
linearization  techniques.  A  direct  adaptive  controller  is  then  developed  through  the  linear 
parameterization  of  the  system  dynamics,  and  the  implementation  of  a  Kalman  Filter 


based  adaption  law.  The  controllers  are  then  simulated  and  compared  for  various  levels 
of  system  parameter  uncertainty.  The  adaptive  controller  is  found  to  be  superior  to  the 
non-adapiive  controllers  for  high  levels  of  system  parameter  uncertainty.  The  non-adaptive 
controller  is  found  to  compare  favorably  to  the  adaptive  controller  in  some  areas  for  low 
values  of  system  parameter  uncertainty.  The  non-adaptive  controller  is  implemented 


experimentally,  consistent  with  hardware  constraints.  Experimental  results  verify  the  need 
for  adaptive  control  when  system  dynamics  are  present  which  have  not  been  modelled. 
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I.  INTRODOCTION 


A.  BACKGROUND 

Robotic  systems  have  been  utilized  to  perform  a  wide 
variety  of  functions  for  many  years.  Their  speed,  precision 
and  reliability  make  them  well  suited  for  applications  ranging 
from  routine  manufacturing  processes  to  interplanetary  space 
exploration.  As  mankind  seeks  to  reach  out  to  other  worlds, 
the  robot  will  play  a  crucial  role. 

Space  based  robotic  systems  are  required  to  deal  with  some 
unique  conditions  which  are  not  encountered  by  their 
terrestrial  counterparts.  The  absence  of  a  fixed  base  upon 
which  to  mount  a  robotic  manipulator  and  the  lack  of 
significant  external  friction  to  dampen  the  system  require 
special  consideration  by  a  control  systems  engineer.  An 
effective  control  system  must  not  only  reposition  the 
manipulator,  but  counteract  the  forces  imparted  on  the  main 
body  by  such  a  maneuver.  This  problem  is  further  complicated 
when  the  space  based  manipulator  is  called  upon  to  handle  an 
object  with  unknown  mass  and  inertia  properties. 

Robotic  systems  equations  of  motion  can  be  developed 
through  Lagrange's  equations  and  are  highly  non-linear. 
Standard  linear  control  techniques  are  not  well  suited  to  this 
kind  of  model.  One  approach  to  the  control  problem  is  to  use 
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feedback  linearization.  This  technique  involves  the 
development  of  a  linearizing  controller  to  cancel  system  non- 
linearities.  Linear  control  techniques  are  then  applied  to  the 
linearized  system. 

When  system  parameter  uncertainty  is  present,  controller 
performance  can  be  improved  through  the  use  of  adaptive 
control  in  which  the  uncertain  system  parameters  are  estimated 
on-line.  Research  in  adaptive  control  started  in  the  early 
1950 's  in  connection  with  the  design  of  autopilots  foi  high- 
performance  aircraft.  Practical  applications  in  robotic 
control  emerged  in  the  1980' s.  Initial  research  relied  on 
restrictive  assumptions  or  approximations  including 
linearization  of  robot  dynamics,  decoupling  assumption  for 
joint  motors  and  slow  variation  of  the  inertia  matrix  [Ref.  1] 
[Ref.  2] [Ref.  3] [Ref.  4].  Later  research  resulted  in  the 
linear  parameterization  of  robot  dynamics  allowing  the 
adaptive  controller  to  fully  account  for  the  non-linear,  time- 
varying  and  coupled  nature  of  robot  dyneunics  [Ref.  5]  [Ref.  6] 
[Ref.  7]  . 

Controllers  for  the  NPS  Spacecraft  Robotic  Simulator  (SRS) 
were  first  developed  by  Sorenson  [Ref.  8]  and  later  modified 
by  Yale  [Ref.  9]  .  Both  developments  assumed  perfect  knowledge 
of  robotic  system  parameters. 
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B.  OVERVIEW  AND  OBJECTIVES 

The  focus  of  this  research  is  to  develop  an  adaptive 
controller  for  the  NPS  SRS  and  to  experimentally  verify  non- 
adaptive  controller  characteristics.  The  equations  of  motion 
and  control  laws  are  developed  in  Chapter  II.  Chapter  III 
contains  a  comparison  of  controller  performances  for  varying 
levels  of  parameter  uncertainty.  Non-adaptive  controller 
experimental  implementation  is  discussed  in  Chapter  IV. 
Chapter  V  includes  a  summary  of  the  conclusions  as  well  as 
topics  for  future  research. 
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II.  ANALYTICAL  MODEL 


The  analytical  model  used  in  this  research  represents  a 
spacecraft  with  a  two-link  manipulator  attached.  The 
manipulator  is  maneuvered  by  motors  at  its  shoulder  and  wrist 
while  a  momentum  wheel  holds  the  spacecraft  centerbody  steady 
in  a  desired  orientation.  Motion  of  the  system  is  confined  to 
two  dimensions  and  the  spacecraft  centerbody  is  allowed  to 
rotate  but  not  translate.  These  restrictions  facilitate 
comparison  between  analytical  and  experimental  results. 

A .  COORDINATE  SYSTEMS 

An  overall  system  schematic  is  shown  in  Figure  1.  This 
diagram  presents  the  system  coordinate  frames  used  to  develop 
the  equations  of  motion.  The  coordinate  frames  utilized  are 
the  same  as  those  utilized  by  Yale  [Ref.  9:pp  7-9]  and  are 
illustrated  in  Figure  1.  The  centerbody  and  manipulator  links 
are  assumed  to  be  rigid  bodies.  Therefore,  member 

lengths  (Li,  Lj)  ,  distances  to  centers  of  mass  (Leo,  ,  Lc2)  , 
the  location  where  the  manipulator  attaches  to  the 

centerbody  (Lq,  Go)  remain  constant.  An  inertial  axis  system  is 
located  on  the  centerbody  at  the  point  of  rotation.  A  body 
fixed  coordinate  frame  uses  the  same  origin  as  the  inertial 
frame  but  rotates  with  the  spacecraft  centerbody.  The  x-axis 
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of  this  frame  points  to  the  centerbody  center  of  mass.  The 
centerbody  attitude,  0o,  is  the  angle  between  the  inertial  x- 
axis  and  the  spacecraft  centerbody  x-axis.  Each  manipulator 
has  its  own  set  of  body  axes.  A  coordinate  frame  attached  to 
the  manipulator  shoulder  aligns  its  x-axis  along  the 
longitudinal  axis  of  manipulator  Link  1.  The  attitude  of  this 
link,  01,  is  zero  when  the  link  lies  on  a  ray  extending  from 
the  inertial  origin  through  the  shoulder.  The  attitude  of  Link 
2  is  defined  by  a  coordinate  frame  attached  to  the  elbow.  The 
attitude  of  this  link,  02,  is  zero  when  the  link  is  parallel 
with  Link  1.  A  set  of  generalized  coordinates,  a,  is  chosen 
which  describe  the  system  include  the  centerbody  attitude  and 
joint  angles  for  both  manipulator  links. 

[600102]  (1) 
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Figure  1:  System  Schematic 


B.  EQUATIONS  OF  MOTION 


The  equations  of  motion  for  this  system  are  a  special  case 
of  those  developed  by  Yale  [yale93:pp.  9-24]  and  are  derived 
using  Lagrange's  equations  for  a  dynamic  system. 


dt\^)  ^ 


(2) 


where : 

•  L=T-V; 

•  T  is  kinetic  energy. 

•  V  is  potential  energy. 

•  q  is  the  generalized  coordinates  vector. 

•  q  is  the  generalized  velocities  vector. 

•  2  is  the  vector  of  applied  non-conservative  forces. 

Using  Lagrange's  equations,  the  equations  of  motion  can  be 
expressed  in  an  alternate  form. 

da 

where : 

•  M  is  a  3x3  inertia  matrix. 

•  G  is  a  3x1  vector  which  accounts  for  centripetal  and 
Coriolis  torques, 

•  V  is  the  potential  energy  of  the  system. 
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Eq. (4)  can  be  further  simplified  because  the  potential  energy 
of  the  system  is  constant,  and  it  becomes 

(4) 

The  following  sections  develop  expressions  for  the  inertia 
matrix,  M, Coriolis  vector,  G,  and  generalized  force  vector  Q. 

1.  Inertia  Matrix,  M 

The  inertia  matrix  is  found  by  calculating  the  system 
kinetic  energy  and  expressing  it  in  the  form 

(5) 

The  total  system  kinetic  energy,  T,  is  the  sum  of  the  kinetic 
energy  of  all  system  components. 

r=ro+ri+r2  (6) 

Where  To,Tj  and  Tj  are  the  kinetic  energies  of  the  centerbody, 
Link  1  and  Link  2  respectively.  Kinetic  energy  of  individual 
components  can  be  found  from 

=  (7) 

where : 

•  li  is  the  member  moment  of  inertia  about  its  center  of 
mass . 

•  (Oi  is  the  member  angular  velocity. 

•  m^  is  the  member  mass. 

•  r  is  the  inertial  velocity  of  the  member  center  of  mass. 
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Kinetic  energies  for  individual  members  were  determined  by 
Yale  [Yale93:  pp  x2-14]  and  are  contained  in  Appendix  A. 

After  substituting  the  expressions  for  kinetic  energy 
obtained  from  Eqs.  (6)  and  (7)  into  Eq.  (4)  ,  the  inertia  matrix, 
M,  is  determined  and  is  of  the  form 

-Wll  ^2  ^3 

M-Mzx  M22  <8> 

.^^31  W32  ^3. 

Expressions  for  the  individual  elements  in  the  inertia  matrix 


are  given  by 

^23  “'^32~^33  (10) 

Mj^3=W3j*A^3+m2lgl^2®OS  (0^+02)  (11) 

^22  =^23  +  ^  1  C2COS02  +ini  i  +in2  i  1  (12) 

Afi2  ="'21=^22+10  cos0i+in2Joic.^cos  (0i+02)  (13) 


Mii=M22+Io*niolco*  Jo +2Jo  (milp^+ii^Ji)  cos0i+2/n2Joie2COS  (01+021 

(14) 

2.  Centripetal  and  Coriolis  Vector,  G 

The  Coriolis  vector,  G,  contains  all  of  the 
centripetal  and  Coriolis  terms  and  may  be  found  using 
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(15) 


where  the  elements  C'"'  are  defined  by  the  Christoffel  symbol 
[Yale93:  p  17] 


(16) 


The  G  vector  for  the  system  is  of  the  form 

G^lG^G^Gjf  (17) 

Expressions  for  individual  elements  of  the  G  vector  are  given 
by 

-mjigica  (260  (61+62)  +  (61+63)  *)  sin  (61+62) 


G2=1o6o  (miJ<.i+m2-ii)  (260+261+63)  sin62 

+m2-io-^c2^oSin(6i+62) 

G3=;;^JiJ^2(6i+62)='sin62+in2Joi^26§sin(6i+62)  (20) 

3.  Generalized  Forces,  ^ 

It  is  beneficial  to  express  the  vector  of  generalized 
forces,  Q,  in  terms  of  torque  vector  U,  representing  torques 
applied  by  individual  actuators.  This  is  accomplished  using 
the  principle  of  virtual  work.  The  total  virtual  work  is  the 
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sum  of  the  torques  applied  to  each  system  component  multiplied 
by  each  component's  virtual  angular  displacement. 


3 

(21) 

The  local  torque  vector  is  simply  a  3  by  1  vector  consisting 
of  torques  applied  by  the  centerbody,  shoulder,  and  elbow 
actuators  respectively. 


Because  the  angles  describing  the  system  are  expressed  in 
local  coordinates  each  actuator  creates  a  virtual  displacement 
only  for  its  associated  component. 

(24) 

This  yields  the  relation, 

Q=U.  (25) 

4.  Equations  of  Motion:  Expressed  in  Local  Coordinates 

Substituting  Eqs . (9) - (14) ,  (18) - (20)  and  (25)  into 
Eq. (4)  produces  the  system  equations  of  motion  expressed  as  a 
function  of  local  coordinates 

=ir  (26) 

Where, 
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<27) 

Eq. (26)  is  the  equation  of  motion  upon  which  system  control 
laws  are  developed. 

C .  REFERENCE  MANEUVER 

Controller  torques  will  be  designed  to  cause  the  system  to 
follow  a  desired  reference  trajectory.  Given  a  three  degree  of 
freedom  system,  one  needs  only  to  specify  the  trajectory  to  be 
followed  by  three  of  the  system's  generalized  coordinates.  The 
remaining  set  of  generalized  coordinate  trajectories  can  be 
found  via  geometric  reasoning. 

1.  Selection  of  Reference  Trajectory 

The  three  coordinates  chosen  to  be  specified  by  the 
reference  trajectory  are  the  actuator  tip  x  and  y  coordinates, 
R*  and  Ry,  and  the  centerbody  angle  Bq.  The  generalized 
coordinates  used  in  the  system  equations  of  motion,  Eq.{26), 
are  Bq,  Bj  and  Bj.  R,  and  Ry  can  be  expressed  in  terms  of  Bq,  Bj 
and  Bj. 

The  choices  between  reference  trajectories  which  move 
the  system  from  a  given  initial  condition  to  a  desired  final 
condition  are  infinite.  To  help  ensure  that  the  spacecraft 
structure  does  not  experience  any  unnecessary  jerks  or 
excitation  of  flexible  structures,  Sorenson  utilized  a  fifth 
order  polynomial  with  zero  velocity  and  acceleration  at 
initial  and  final  conditions  [Sore:  pp  25-29]. 


L2 


f (t) =6t5-15t<+10t3 


(28) 


Where  the  normalized  time,  x,  is  defined  as 


t  = 


tf-tp 


(29) 


Given  the  desire  to  hold  the  centerbody  attitude  constant 
during  a  given  manipulator  maneuver,  the  centerbody  angle, 
angular  velocity  and  angular  acceleration  reference  maneuvers 
are  simply 

00=0 

60=0  (30) 

00=0 

The  manipulator  tip  position  velocity  and  acceleration  (R,  R, 
R)  are  found  via 


Eix]  =£(Co)  *f{x)  iEitf)  -EitJ] 

Cg 


Where  R(X)  is  the  position  vector  originating  at  the 
centerbody  point  of  rotation  and  ending  at  the  tip  of  Link  2. 
2 .  Coordinate  Transformation 

The  system  control  laws  to  be  developed  will  require 
angular  position,  velocity  and  acceleration  of  all  elements  of 
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the  system  generalized  coordinate  vector  fl.  These  can  be  found 

geometrically . 

Generalized  coordinates  B^R^and  Ry  along  with  their 
respective  velocities  and  accelerations  have  been  derived  in 
the  previous  section. 


Figure  2:  Manipulator  Joint  Angle  Derivation  Schematic 
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The  position  of  the  shoulder  (S*.  Sy)  and  the  magnitude  of  the 
vector  between  the  shoulder  and  the  tip  of  Link  2  (SR)  can 
then  be  expressed  as 

S^=locosido+6g) 

S^=2oSin{eo+e^)  (32) 

SR=^ {Ry-Sy)^ 

Angles  formed  by  the  triangle  formed  by  SR  and  the  two 
manipulator  links  are  found  using  the  law  of  cosines. 


Pi=ata 


(33) 


The  local  angles  are  then  found  to  be 


(34) 


(35) 


0i  =  Pi-P2-(eo+0J  (36) 

e2=180”-P3  (37) 

Manipulator  joint  velocities  and  accelerations  are  found  by 
expressing  the  manipulator  end  point  coordinates  in  terms  of 
system  generalized  coordinates. 


15 


J?j^=iocos  (0^+60)  +JiCos  (6.+6o+0i)  +i2C08  (0S+0O +01+02)  (38) 


Ry=lo3in{Qg*Q^)  +iiSin{0^+0Q+0i)  +J28in  (0^+00+01+02)  (39) 

Upon  differentiation  these  equations  can  be  expressed  in  the 
form 


(40) 


Where  H  is  the  Hamiltonian  matrix  expressed  in  the  form 


"’ll 


^12 

■^22. 


Individual  elements  of  H  are  found  to  be 


(41) 


/fn  =  -i2sin(0g+0o+0i+02)  -J!iSin(0g+0o+0i) 

i/12  =  --Z2sin  (0^+00+01+02) 

//2i=J2COS  (0^+00+01+02)  +I1COS  (0^+00 +01 ) 
H22  =  i2COS  (0^+00+01+02) 


(42) 


On  the  basis  of  Eg. (40) joint  velocities  are  then  found  as 


(43) 


together  with  the  joint  accelerations 
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r.R  j 

fOil 

kJ 

kJ 

kJ 

Rearranging  Eq. (44)  we  obtain 


(44) 


(45) 


3 .  Reference  Torques 

Given  the  reference  values  for  the  system  generalized 
coordinates  and  their  velocities  and  accelerations,  a 
reference  torque,  Ur,  can  be  derived  which  would  produce 
perfect  tracking  in  the  case  of  no  external  disturbances  or 
modelling  errors.  The  derived  reference  torque  alone  would 
represent  an  open  loop  type  controller. 

The  system  reference  torque  is  derived  by  evaluating 
the  system  equations  of  motion  at  the  reference  values  of  the 
system  generalized  coordinates  and  their  higher  order 
derivatives  as  presented  in  Eq. (26) . 


D.  NON -ADAPTIVE  CONTROL  LAW  DESIGN 

In  this  section  two  non-adaptive  control  laws  are 
developed  for  the  space  based  robotic  manipulator.  The  first 
uses  feedback  linearization  to  cancel  system  non-linearities 
in  conjunction  with  a  PD  type  controller.  The  second  is  a 
modification  to  the  first  in  which  the  non-linear  portion  of 
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the  controller  utilizes  generalized  coordinate  reference 
values  instead  of  state  feedback. 

1.  Linearizing  Controller 
a.  Controller  Design 

The  control  law  presented  utilizes  feedback 
linearization  to  cancel  out  non-linearities  which  occur  in  the 
system  inertia  matrix,  M,  and  Coriolis  vector,  G.  A  PD 
controller  is  then  applied  to  the  linearized  system.  The 
control  law  can  be  expressed  in  the  following  form 

(46) 

The  linearizing  term,  Ul,  serves  to  cancel  system  non- 
linearities  by  predicting  the  current  values  of  the  system 
inertia  matrix  and  Coriolis  vector. 

(47) 

The  PD  control  term,  6u,  corrects  for  tracking  errors 
encountered  by  providing  state  feedback  to  the  system. 


A  controller  block  diagram  is  presented  in  Figure  3. 


I 

i 


(48) 
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Figure  3:  Linearizing  Controller  Block  Diagram 
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b.  Coatrol  Law  Stability 

Control  law  stability  is  determined  by  considering 
the  behavior  of  the  system  trajectory  error,  e.  The  trajectory 
error  and  its  first  and  second  derivatives  are  defined  as 

(49) 

(50) 

(51) 

Substituting  Eqs , (49 ) - ( 51 )  into  Eq. (46)  yields 

ir=G(fl,fi) +Ar(fi)  (52) 

which  in  turn  can  be  combined  with  Eqs  (50)  and  (25)  to  obtain 

//(fl)  =0 

Because  the  system  inertia  matrix  is  positive  definite  and 
thus  invertible,  for  any  positive  definite  and 

sit)  =0. 

2 .  Reference  Controller 

The  reference  controller  presented  is  merely  a  slight 
modification  to  the  linearizing  controller  presented 
previously.  Rather  than  picking  an  inertia  matrix  and  Coriolis 
vector  to  cancel  out  system  non-linearities,  M  and  G  are 
calculated  based  on  where  the  system  should  be  as  determined 
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by  a  desired  reference  trajectory.  The  form  of  the  controller 
is  similar  to  that  of  the  linearizing  controller 


11=11^-611  (55) 

The  PD  control  term,  Su,  is  identical  to  that  presented  in  Eq. 
(48)  .  The  linearizing  term,  1^.,  serves  to  cancel  system  non- 
linearities  by  predicting  the  reference  torques  required  to 
produce  the  desired  reference  trajectory  when  perfect  tracking 
is  assumed. 

(56) 

A  controller  block  diagram  is  presented  in  Figure  4. 

E.  ADAPTIVE  CONTROL  LAW  DESIGN 

Robotic  manipulators  are  designed  in  order  to  grasp  or 
manipulate  an  object.  Often  the  mass  and  inertia  properties  of 
the  object  are  not  known  beforehand.  This  in  addition  to 
modelling  errors  leads  to  uncertainty  in  system  parameters  and 
degraded  control  law  performance.  Adaptive  control  utilizes 
system  input  and  output  data  to  update  the  system  parameters 
and  thereby  adjust  to  changes  in  system  parameters. 

1.  Control  Law  Design 

The  adaptive  controller  developed  is  merely  a 
modification  to  the  non-adaptive  reference  controller 
presented  in  Eq. (55) .  The  only  difference  between  this 
controller  and  the  non-adaptive  version  is  a  dependance  on  an 
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Figure  4:  Reference  Controller  Block  Diagram 
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estimate  of  the  system  parameter  vector  A.  In  the  non-adaptive 
case  the  system  inertia  matrix,  M,  and  G  vector  are  functions 
only  of  the  generalized  coordinates.  In  the  adaptive  case,  M 
and  G  are  functions  of  both  generalized  coordinates  and  the 
system  parameter  vector  estimate  A. 


(57) 


The  meaning  of  A  is  developed  in  the  following  section.  A 
controller  block  diagram  is  presented  in  Figure  4. 

2 .  System  Parameterization 

The  system  parameter  vector  A  is  determined  by 
expressing  Eg. (25)  in  an  alternate  form 


(58) 

where  O  is  a  function  of  the  system  generalized  coordinates 
and  A  is  a  function  of  the  system  parameters.  Equating 
Eqs.{26)  and  (58),  A  is  found  to  be 

A(l) 

A  {2) 

A  ( 3 ) 

(59) 

A  (4) 

A  { 5 )  ~ ig  ^ 

A  (6)  =Io+moi|o+ 


The  matrix  of  generalized  coordinate  0  is  of  the  form 
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Expressions  for  the  individual  elements  of  O  are  given  by 


4>u=eo+0i+02 

®13=«U 

®21=  (200+20^+02)  COS02-02  (200+201+02)  Sin02 
®22"®21 

^23"  COSO2+  (01+02)  ^Sin02 

®31=  (200+01+02) cos  (01+02)  -  (200  (01  +02)  + (01+02) sin  (01+02) 


(61 

(62 

(63 

(64 

(65 

(66 

(67 


4*32=00003(01+02)  +0oSin(Oi+e2) 


(68 


(70) 
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■^41 


®5i=  (20(,+0i)  cosOj-  (0i+20q0i)  sinOi 
®52=0oCOS0i+0oSin0i 


(71) 

(72) 

(73) 

(74) 


3.  Adaption  Law 

The  system  parameter  vector,  A,  is  updated  via  a 
recursive  Kalman  Filter.  The  standard  Kalman  Filter  state 
space  equations  can  be  expressed  in  the  form 


2f(t+l)  =02c(t)  +Aj^(t) 
y.{t)  =Cx{t)  *Y[t) 


Assuming  a  constant  system  parameter  vector  A  for  a  given 
maneuver,  the  system  can  be  expressed  in  state  space  form  as 


Equating  Eqs.(75) 


A(t+1)  =A(t)  +J^(t) 

nit)  =®^(t)A(t)  +z(t) 


(76) 


and  (76)  yields  the  following  relations. 
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r 

I 


X=A 


o*A  = 


10  0 
0  10 
p  0  1, 


(77) 


Applying  standard  Kalman  Filter  equations  to  Eq. (76)  yields 
the  following  set  of  recursive  equations 


A(t+i)  =A(t)  ^K{t)  ] 

/f(t)  =P(t)  4>(t)  [X+«^(t)P(t)«{t)  ] -^ 
p(t+i)  =p(t)  P(t)  *Q 


where 

•  K(t)  is  the  Kalman  Filter  gain. 

•  X  is  the  noise  covariance  matrix,  E[e(t)  e’^(t)]. 

•  P  is  the  parameter  error  covariance  matrix,  E[(A(t)- 

^ccual)  (A(t)  “Actual  • 

•  is  the  plant  noise  covariance  matrix,  E[v(t) v^(t)  ]  . 
Because  the  noise  covariance  matrices,  X  and  Q,  are  not  known, 
the  parameter  error  covariance  matrix,  P,  and  the  plant  noise 
covariance  matrix,  Q,  are  redefined 

P{t) 

-  ^  (79) 

Combining  Eqs.(23)  and  (24)  yields  the  recursive  equations 
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=4(t)  *K(t)  [ii(t)  -®^{t)a(c)  ] 

JC(t)  =P(t)*(t)  [J+®’’(t)P(t)*(t)]'^  <80) 

p(t+i)  =p(c)  -ii:(t)*^(t)p(t)  +0 

Eq. (80)  provides  the  recursive  equations  necessary  to  update 
the  system  parameter  vector,  A. 

In  the  next  chapter  the  three  controllers  are 
implemented  for  various  levels  of  parameter  uncertainty  and 
their  performances  compared. 
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III.  SIMUliATION  RESULTS 


The  computer  simulations  presented  in  this  chapter  were 
obtained  using  the  MATLAB  subroutines  listed  in  Appendix  B. 
Simulations  are  presented  for  various  levels  of  system 
parameter  uncertainty  ranging  from  0%  to  500%.  System 
parameter  values  presented  in  Table  1  correspond  to  actual 
values  of  the  Spacecraft  Robotics  Simulator  and  are  utilized 
to  simulate  system  dynamics.  Parameter  values  used  by  the 
controller  contain  a  random  error  up  to  a  specified  percentage 
of  the  actual  parameter  value. 

Equations  of  motion  and  computer  code  are  verified  by 
examining  the  change  in  angular  momentum  of  the  system  for 
each  simulation.  For  a  given  maneuver  the  rate  of  change  in 
angular  momentum  will  equal  the  sum  of  external  torques  on  the 
system.  The  only  external  torque  experienced  by  the  Spacecraft 
Robotic  Simulator  is  produced  by  the  centerbody  momentum 
wheel.  Thus,  for  each  simulation  the  relation 

(77) 

should  be  satisfied.  Where  H  is  the  rate  of  change  in  angular 
momentum  and  U„hee;  is  the  torque  produced  by  the  centerbody 
momentum  wheel.  The  right  hand  side  of  Eq(77)  was  found  to  be 
^  10  Nm/s^  for  all  simulations. 
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A.  SIMULATION  TEST  CASES 

Simulation  results  are  presented  for  five  cases.  The  first 
case  trains  the  adaptive  control  to  recognize  centerbody 
characteristics.  Cases  2-6  examine  the  effects  of  parameter 
uncertainty  on  a  desired  manipulator  maneuver.  During  the 
maneuver,  the  manipulator  tip  is  repositioned  from  an  initial 
to  a  final  point  along  a  straight  line  between  the  two  points 
as  shown  in  Figure  6.  The  angular  position  of  the  centerbody 
is  held  constant. 

1.  Case  1:  Adaptive  Controller  Training  Maneuver 

Adaptive  parameter  A(6)  depends  only  on  centerbody 
characteristics.  In  order  to  update  this  parameter,  a 
centerbody  maneuver  is  required.  Cases  2-6  attempt  to  hold  the 
centerbody  fixed  and  produces  a  small  centerbody  angular 
position,  velocity  and  acceleration.  A  separate  case,  in  which 
the  centerbody  is  maneuvered,  is  required  to  adaptively  update 
centerbody  characteristics.  During  this  maneuver,  the 
manipulators  are  maneuvered  in  accordance  with  the  reference 
maneuver  pictured  in  Figure  6  while  the  centerbody  is 
maneuvered  as  shown  in  Figure  7.  Once  the  centerbody 
parameter, A{ 6) ,  is  updated,  it  is  assumed  fixed  and  no  error 
is  induced  into  this  parameter  in  Cases  2-6. 
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TABLE  1:  SYSTEM  PARAMETER  VALUES 


Parameter  Name 


Cencerbody  Radius 


Arm  1  Length 


Arm  2  Length 


Centerbody  CM 


Arm  1  CM 


Arm  2  CM 


Centerbody  Mass 


Arm  1  Mass 


Arm  2  Mass 


Centerbody  Inertia 


Arm  1  Inertia 


Arm  2  Inertia 


Parameter  Variable 


Parameter  Value 


0.427  m 


0.530  m 


0.533  m 


0.104  m 


0.403  m 


0.314  m 


65.96  kg 


2.34  kg 


2.86  kg 


5.74  kg-m^ 


0.081  kg-m^ 


0.182  kg-m^ 


Figure  6:  Reference  Maneuver  Time  Lapse  Stick  Figure 
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Figure  7 :  Centerbody  Parameter  Training  Maneuver 
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time(sec) 


2.  Cases  2-6:  0-500%  Parameter  Uzicertainty 

Controller  performance  is  presented  for  various  levels 
of  parameter  uncertainty  ranging  from  0-500%.  Controller 
errors  and  adaptive  parameter  updates  are  presented  in  Figures 
8-23.  Centerbody  control  torque  characteristics  are  presented 
in  Table  2 . 

B.  COMPARISON  OF  CONTROLLERS 

1.  Adaptive  Controller  vs  Non-adaptlve  Controller 

The  adaptive  controller  is  clearly  superior  to  the 
non-adaptive  controllers  for  large  values  of  parameter 
uncertainty  (>50%  parameter  uncertainty) .  For  small  values  of 
parameter  uncertainty,  the  linearizing  controller  is  superior 
to  the  adaptive  controller  in  all  but  centerbody  control. 

2.  Linearizing  Controller  vs  Reference  Controller 

The  linearizing  controller  is  superior  to  the 
reference  controller  for  low  to  moderate  values  of  parameter 
uncertainty  (<150%  uncertainty) .  The  reference  controller 
exhibits  superior  performance  over  the  linearizing  controller 
for  the  case  of  150%  parameter  uncertainty. 
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TABLE  2:  CBNTBRBODY  CONTROL  TORQUE  CHARACTERISTICS 


Controller 

Type/ 

parameter 

uncertainty 

U«heel  Max 

Uwhaei  Min 

— 

Linearizing 

(0%) 

0.3735 

-0.3987 

0.2468 

Reference 

(0%) 

0.3735 

-0.3987 

0.2468 

Linearizing 

(50%) 

0.3723 

-0.3987 

0.2464 

Reference  (50%) 

0.3738 

-0.3980 

0.2472 

Adaptive  (50%) 

0.3735 

-0.3987 

0.2468 

Linearizing 

(100%) 

0.3741 

-0.3963 

0.2489 

Reference 

(100%) 

0.3742 

-0.4000 

0.2490 

Adaptive  (100%) 

0.3735 

-0.3987 

0.2468 

Linearizing 

(150%) 

0.3736 

-0.3958 

0.2496 

Reference 

(150%) 

0.3744 

-0.3930 

0.2491 

Adaptive  (150%) 

0.3734 

-0.3987 

0.2470 

Adaptive  (500%) 

1.6784 

-2.1698 

0.2659 
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Error  (mm)  Error  (decrees)  |  I  Error  (mm)  Error  (degrees) 


Figure  8:  Linearizing  Controller  Error 
(0%  pareuneter  uncertainty) 


Figure  9:  Reference  Controller  Error 
(0%  parameter  uncertainty) 


35 


Enw  (mni)  Error  (degrees)  I  |  Error  (mm)  Error  (degran) 


Figure  10:  Linearizing  Controller  Error 


(50%  parameter  uncertainty) 


Figure  11:  Reference  Controller  Error 
(50%  parameter  uncertainty) 


36 


Paromoler  Values  I  I  Error  (mm)  Error  (degrees) 


Figure  12:  Adaptive  Controller  Error 
(50%  parameter  uncertainty) 


0.7 

0.«9 

0.6 

0.55 

0.5 

O.A5 

0.4 

0.35 


Figure  13:  Adaptive  Parameter  Updates 


(50%  parameter  uncertainty) 
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Error  (mm)  Error  (degrees)  I  |  Error  (mm)  Erry  (degrees) 


Figure  14:  Linearizing  Controller  Error 
(100%  parameter  \incertainty) 


Figure  15:  Reference  Controller  Error 
(100%  parameter  \incertalnty) 
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Paromeier  Vokies  I  |  Error  (mm)  Error  (degrees) 


Figure  16:  Adaptive  Controller  Srror 


(100%  parameter  uncertainty) 


O.B 

0.79 

0.7 

0.65 

0.6 

0.55 

0.5 

0.*5 

O.* 

0.35 

0.3 


Figure  17 :  Adaptive  Parameter  Updates 


(100%  parameter  uncertainty) 
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Error  (mm)  Errqr  (degrees)  |  I  Error  (mm)  Error  (degrees) 


Figure  18:  Linearizing  Controller  Error 
(150%  parameter  uncertainty) 


Figure  19:  Reference  Controller  Error 
(150%  pareuneter  uncertainty) 
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Porometer  Volues  I  I  Error  (mm)  Error  ((tegrees) 


Figure  20:  Adaptive  Controller  Error 
(150%  parameter  \mcertalnty) 


Figure  21:  Adaptive  Parameter  Updates 
(150%  parameter  uncertainty) 
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Poromrt«r  Vokies  |  I  Error  (mm)  Error  (degrees) 


Figure  22:  Adaptive  Controller  Error 
(500%  parameter  uncertainty) 


Figure  23:  Adaptive  Parameter  Updates 


(500%  parameter  uncertainty) 
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IV.  EXPERIMENTAL  WORK 


The  spacecraft  Robotics  Simulator  (SRS)  was  utilized  for 
the  experimental  portion  of  the  thesis.  The  SRS  is  a 
derivative  of  the  Flexible  Spacecraft  Simulator  (FSS) 
initially  developed  by  Watkins  [Ref.  10]  and  later  modified  by 
Hailey  [Ref.  11] .  Sorenson  [Ref.  8]  began  the  work  to  convert 
the  FSS  into  the  SRS.  The  robotic  manipulator  utilized  was 
developed  by  Yale  [Ref.  9]. 

A.  SETUP 

The  SRS  permits  experimental  investigation  of  two- 
dimensional  robotics  motion  and  rotational  spacecraft  dynamics 
and  is  illustrated  in  Figures  24  and  25.  The  simulation 
hardware  is  floated  on  an  eight  foot  by  six  foot  granite  table 
by  means  of  a  thin  layer  of  air  supplied  by  an  external 
source.  The  table  is  polished  to  within  0.001  inch  peak  to 
valley  and  leveled  to  prevent  gravitational  accelerations  from 
impacting  the  motion  across  its  surface.  The  following 
sections  describe  the  simulated  spacecraft  with  its  associated 
sensors  and  actuators  and  the  controller  which  together  form 
the  SRS.  The  spacecraft  components  are  the  centerbody  and  two- 
link  robotic  manipulator. 
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Figure  25;  SRS  Top  View 


1 .  Centerbody 

The  centerbody  is  a  30  inch  diameter,  7/8  inch  thick 
aluminum  disk.  It  carries  a  position  sensor,  rate  sensor, 
momentum  wheel,  thrusters,  and  an  air  tank  to  power  the 
thrusters.  The  centerbody  also  serves  as  the  mounting  platform 
for  the  manipulators  and  is  floated  by  a  control  air  bearing 
and  three  air  pads  located  at  120  degree  intervals  near  the 
outer  edge.  The  air  pads  are  each  capable  of  floating  60 
pounds  when  the  air  pressure  supplied  to  the  pads  is  80  psi . 
The  centerbody  is  allowed  to  float  freely  on  the  granite 
t-  able . 


45 


The  centerbody  angular  rate  is  measured  by  a  rate 
transducer  manufactured  by  Humphrey,  Inc,,  having  a  range  of 
±100  deg/sec  and  a  minimum  threshold  of  0.01  deg/sec.  The 
centerbody' s  translation  is  not  measured  and  is  neglected  for 
the  experiment . 

The  centerbody 's  angular  position  is  controlled  by  a 
momentum  wheel  and  is  powered  by  a  model  JR16M4CH/F9T  servo 
motor  manufactured  by  PMI  Industries  whose  characteristics  are 
summarized  in  Table  3,  Although  the  centerbody  also  carries 
two  thrusters,  they  are  not  used  in  this  research, 


TABLE  3:  MOMEMTUM  WHEEL  MOTOR  CHARACTERISTICS 


n  Manufacturer 

PMI  Industries 

1  Model 

JR16M4CH/F9T 

1  Rated  Output  Speed  (rpm) 

3000 

1  Rated  Current  (amps) 

7.79 

1  Rated  Voltage  (volts) 

168 

Rated  Torque  (in- lb) 

31.85  1 

Height  (in) 

4.5  1 

Weight  (lb) 

175  1 

Outside  Diameter  (in) 

7.4  1 

2 .  Manipulators 

The  two-link  manipulator  has  three  joints.  The 
shoulder  joint  is  supported  by  the  centerbody  while  the  elbow 
and  wrist  joints  are  supported  by  two  air  pads  apiece.  The 
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links  between  the  joints  are  stiff  laterally  but  permit  some 
flexibility  vertically.  This  feature  increases  the  tolerances 
on  the  air  pad  height  adjustment.  Joint  angles  for  the 
shoulder  and  elbow  are  sensed  by  encoders  purchased  with  the 
joint  actuators.  The  encoder  resolution  is  0.005  degrees. 
Manipulator  actuators  are  harmonic  drive  dc  servo  actuators 
manufactured  by  HD  Systems,  Inc.  The  shoulder  actuator  is 
model  RFS-25-6018-E036AL  while  the  elbow  and  wrist  actuators 
are  model  RFS-20-6012-E036AL.  Specifications  for  joint 
actuators  are  contained  in  Table  4.  The  wrist  joint  actuator 
and  sensor  is  not  utilized  in  this  experiment.  Manipulator 
schematics  are  contained  in  Figure  26. 

The  joint  actuators  are  all  driven  by  Kepco  power 
supplies.  These  bipolar,  programmable,  linear  amplifiers  can 
be  controlled  manually  from  the  front  panel  or  controlled 
remotely  with  a  ±10  volt  signal.  In  this  application.  The 
power  supplies  are  operated  in  the  current  control  mode  with 
the  voltage  and  current  limits  manually  set  consistent  with 
the  values  in  Table  4.  The  specific  power  supply  models  and 
their  characteristics  are  summarized  in  Table  5. 
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TABLE  4:  MANIPULATOR  ACTUATOR  CHARACTERISTICS 


Figure  26:  Manipulator  Top  and  Side  Views 
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3.  Controller 


The  AC-100  programmable  controller  manufactured  by 
Integrated  Systems,  Inc.  controls  the  SRS.  The  AC-100  includes 
an  Intel  80386  Application  Processor,  an  Intel  80386  Multibus 
II  Input/Output  Processor,  an  Intel  80386  Communication 
Processor,  an  Intel  80387  Coprocessor,  a  Weitek  3167 
Coprocessor,  an  Analog-To-Digital  and  Digital-To-Analog  Data 
Translation  DT2402  Input/Output  Board,  two  INX-04  Encoder  and 
Digital-To-Analog  Servo  Boards,  and  an  Ethernet  Interface 


TABLE  5:  POWER  SUPPLY  CHARACTERISTICS 


Model 

BOP  72-6M 

BOP  72-3M 

Actuator  Controlled 

Right  Shoulder 

Right  Elbow 

DC  Output  Range 

±72  volts  ±6  amps 

±72  volts  ±3  amps 

Closed  Loop  Gain 

0.6  (amp/volt) 

0.3  (amp/volt) 

Module.  The  AC-100  also  includes  software  installed  on  a  VAX- 
3100  Series  Model  30  workstation.  The  software  permits  design 
of  a  controller  in  block  diagram  graphical  form  and  conversion 
of  the  diagram  to  C  language  programming  code.  The  user  is 
also  able  to  design  an  interactive  animation  window  to  operate 
the  controller.  The  AC-100  receives  input  signals  from  the 
sensors  and  the  graphical  user  interface.  AC-100  output 
signals  go  to  the  power  supplies  driving  the  actuators  or  to 
the  graphical  user  interface  for  display. 
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4.  System  integration 


Several  problems  were  encountered  while  attempting  to 
implement  the  non-adaptive  reference  controller 
experimentally.  As  a  result  several  modifications  to  the 
experiment  were  made. 

a.  Actuator  Dead  Zones 

The  first  problem  was  caused  by  the  large  dead 
zones  present  in  the  harmonic  drive  motors .  Both  the  shoulder 
and  wrist  motors  were  designed  to  be  operated  in  a  high  torque 
environment.  The  SRS  components  are  relatively  small  and  offer 
little  resistance  to  motion.  This  resulted  in  reference 
torques  for  a  reasonable  maneuver  which  were  entirely  within 
the  dead  zone  of  system  actuators.  This  caused  system  tracking 
errors  to  build  up  until  the  PD  control  term  produced  torques 
larger  than  the  actuator  dead  zones. 

Jb.  Centerbody  Resistance 

A  second  problem  involved  a  noticeable  resistance 
to  rotation  by  the  centerbody.  This  is  due  in  part  to  the 
inability  of  the  central  air  bearing  to  function  except  under 
very  low  lateral  loading.  This  lateral  loading  was  eliminated 
by  allowing  the  centerbody  to  float  freely  and  ignoring 
translation  of  the  centerbody.  This  decreased  some  centerbody 
resistance  to  rotation  but  some  resistance  was  still  detected 
due  to  the  effects  of  external  wiring  necessary  for  centerbody 
components . 
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c.  MQmentuJB  Mheel  Coatrol 

A  third  problem  involved  the  ability  to  safely 
control  the  centerbody  reaction  wheel.  The  AC-100  control 
software  was  found  to  be  subject  to  periodic  freeze  ups  in 
which  controller  outputs  could  not  be  reliably  predicted.  The 
momentum  wheel  could  therefore  not  be  safely  utilized.  The 
experimental  modification  was  to  perform  runs  in  which  the 
centerbody  was  held  fixed  to  simulate  perfect  reaction  wheel 
performance . 

B .  RESULTS 

Results  are  presented  in  Figures  27-40  for  four  control 
cases . 

1.  Case  1:  High  Gain,  Free  Centerbody 

This  case  utilizes  a  high  gain  controller  to  control 
a  system  in  which  centerbody  motion  is  not  constrained.  The 
controller  gains  utilized  are  presented  in  Table  6. 

2.  Case  2:  Low  Gain  Controller,  Free  Centerbody 

In  this  case,  a  low  gain  controller  is  utilized  to  a 
control  a  system  in  which  the  centerbody  motion  is  not 
constrained.  The  controller  gains  utilized  for  this  case  are 
presented  in  Table  6. 
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3.  Case  3:  High  Gain  Controller,  Fixed  Centerbody 


The  controller  utilized  in  this  case  is  identical  to 
that  of  Case  1  but  the  centerbody  is  now  held  in  a  fixed 
position . 

4.  Case  4:  Low  Gain  Controller,  Fixed  Centerbody 

This  controller  is  identical  to  Case  2  but  is  used  to 
control  a  system  in  which  the  centerbody  position  is  held 
fixed  as  in  Case  3. 

C.  COMPARISON  OF  CONTROLLERS 

1.  High  vs  Low  Gain 

The  high  gain  controllers  yielded  lower  steady  state 
errors  than  the  low  gain  controllers  at  the  cost  of  larger 
oscillations  during  the  maneuver. 

2.  Free  vs  Fixed  Centerbody 

When  the  centerbody  is  allowed  to  float  freely, 
significant  errors  in  centerbody  attitude  and  manipulator  tip 

position  are  seen. 

TABLE  6:  CONTROLLER  GAINS 


Shoulder  Gain 

Elbow  Gain 

Kp  (low  gain 
controller) 

500 

1000 

Kp  (high  gain 
controller) 

1000 

2000 

K„  (low  gain 
controller) 

10 

25 

K^,  {high  gain 
controller) 


20 
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Figure  27:  9o  Tracking  Performance 
(High  Gain,  Free  Centerbody) 
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Figure  29:  0,  Tracking  Performance 
(High  Gain,  Free  Centerbody) 


Figure  30;  Tip  Tracking  Performance 
(High  Gain,  Free  Centerbody) 
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Figure  31:  0,  Tracking  Performance 
(Low  Gain,  Free  Centerbody) 
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Figure  32;  Gj  Tracking  Performance 
(Low  Gain,  Free  Centerbody) 
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Figure  33:  0j  Tracking  Performance 


(Low  Gain,  Free  Centerbody) 
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Figure  34;  Tip  Tracking  Performance 
(Low  Gain,  Free  Centerbody) 
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Figure  36:  0,  Tracking  Performance 
(High  Gain,  Fixed  Centerbody) 
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Figure  37 ;  Tip  Tracking  Performance 
(High  Gain,  Fixed  Centerbody) 
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Figure  38:  9i  Tracking  Performance 
(bow  Gain,  Fixed  Centerbody) 
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<Low  Gain,  Fixed  Centerbody) 
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V.  SUMMARY  AND  CONCLUSIONS 


A.  SUMMARY 

Three  controllers  were  successfully  developed  for  a  spaced 
based  two- link  robotic  manipulator.  Two  non-adaptive 
controllers  (a  linearizing  controller  and  a  reference 
controller)  were  first  developed  using  feedback  linearization 
techniques.  An  adaptive  controller  was  then  developed  through 
the  linear  parameterization  of  system  dynamics  and  the  use  of 
a  recursive  Kalman  Filter  based  adaption  law.  Controllers  were 
then  compared  for  system  parameter  errors  up  to  ±500%. 

Centerbody  pointing  accuracy  was  improved  by  utilizing 
adaptive  control  while  centerbody  control  torque  was  effected 
very  little.  For  high  values  of  parameter  uncertainty, 
manipulator  tracking  errors  were  smaller  when  using  the 
adaptive  controller.  For  low  values  of  parameter  uncertainty, 
the  linearizing  non-adaptive  controller  outperformed  the 
adaptive  controller  in  some  areas. 

Implementation  of  the  non-adaptive  reference  controller 
experimentally  demonstrated  the  effects  of  un-modelled  system 
dynamics.  The  oscillations  and  steady  state  errors  encountered 
only  reinforced  the  value  of  adaptive  control  in  real  world 
applications.  High  PD  control  gains  produced  larger 
oscillations  but  smaller  steady  state  errors  than  low  gains. 
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Manipulator  maneuvers  produced  significant  disturbances  on 
centerbody  attitude  when  no  control  was  applied  to  hold  the 
centerbody  steady. 

B.  REC(»(MBNDATIOMS  FOR  FURTHER  STUDY 

Feedback  linearization  represents  only  one  way  to  attack 
a  non-linear  control  problem.  Other  approaches  include  neural 
networks  and  sliding  mode  controllers. 

The  adaptive  controller  developed  may  be  too  cumbersome  to 
implement  in  real  time.  In  order  to  decrease  computation  time 
the  system  can  be  re-parameterized  in  terms  of  only  payload 
characteristics  and  a  more  efficient  adaption  law  developed. 

Experimental  implementation  inaccuracies  resulted  f rom  un- 
modelled  actuator  dead  zones.  System  equations  of  motion  and 
reference  torques  can  be  reformulated  to  include  this  effect. 
The  use  of  fuzzy  logic  is  also  worth  investigating. 
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APPENDIX  A:  MEMBER  KINETIC  ENERGIES 

Kinetic  energy  of  individual  components  is  found  using 

Eq . ( 7 ) 

(7) 

The  centerbody  angular  rate  and  center  of  mass  position  vector 
are  given  by 

«o=eo  (79) 

(80) 

Differentiating  Eq. (80)  results  in  the  velocity  of  the 
centerbody  center  of  mass 

~^co^o9o  (81) 

Substituting  Eq.(79)  and  (81)  back  into  Eq.(7)  and  collecting 
on  the  angular  rate  term  leads  to 

=  (82) 

Similar  developments  are  used  for  each  of  the  remaining 
pieces  in  the  system.  For  the  manipulator  link  between  the 
shoulder  and  elbow  (Link  1),  the  angular  velocity  is  a 

combination  of  centerbody  rotation  and  rotation  of  the  link 
with  respect  to  the  centerbody. 
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(83) 


6>i=6o+6i 

The  position  vector  to  the  center  of  mass  is 


X^=I,oCOs6(jJ?o+LoSineo^o+Lpj^i  (84) 

The  first  two  terms  in  the  position  vector  represent  the 
location  of  the  shoulder.  Differentiating  the  position  vector 
gives  the  expression  for  the  velocity  vector.  Because  none  of 
the  coordinate  axes  used  in  the  position  vector  expression  are 
inertial,  their  rotation  must  be  included  as  well. 

i^=LoCOS0o5^o“-^o“oSineo.^o+-^ej“i.^^i  <  85 ) 

After  Eqs . (83)  and  (84)  are  substituded  into  Eq. (7)  and  terms 
are  grouped  by  angular  rates,  the  Icinetic  energy  is 

ri=6o  (0 . 5  di+miLlj+nJiLo )  (sinOpSin (0(,+6i)  +cos  {0o+0i) ) ) 

+  O.50^(Ji+i7?iL|j) 

■*-&QQ^{I^+m^Lci-*-inj^LgLcj{sindgSin[dQ*Qi)  +cos0ocos  {0o+0i) ) ) 

(86) 

The  angular  rates  for  the  link  between  the  shoulder  and  wrist 
(Link  2)  includes  the  centerbody  angular  rates  as  well  as  the 
angular  rates  of  the  body  axes  on  Links  1  and  2 . 

0)2  '*'^2  (87) 

This  link's  center  of  mass  position  vector  is 

^2  =LoCOS0oi?o +-f'oSin0o:J>o *^^2^2  ( 88 ) 

Differentiating  the  position  vector  gives  the  velocity  vector 
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4  =I.o«oCOseo^o  -io<«>oSin6o-^o  ( 89  > 

The  kinetic  energy  for  link  2  is  found  after  substituting 
Eqs .  (87)  and  (89)  into  Eq.  (6)  and  collecting  terms  with  common 
angular  rates . 


r2=e^  (0 . 5 

^io^LqL^  (sin6Qsin(0o+6i)  +cos0ocos  id^+Q^)  +;7^LiI.p2Cos02 
+m2l-oLc2(sin0osin  (00+01+02)  +cos0ocos  (0o+0i+02) ) ) 

+01  (0 . 5  [I^^ni^Ll^m^Lls)  +ii^XiI.^2COS02)  +0 . 502  ( J2+m2L|2) 
+0001  ( l2+m2l.i +in2Lj2+2m2LiL^2COS02 
+m2l'o-^i  (sin0oSin(0o+0i)  +cos0ocos  (0o+0i) ) 
+m2l'o-^'c2(sin0oSin  (00+01+02)  +cos0ocos  (0o+0i+02) ) ) 
+0002  (  ^2  +^i|2+m2-^l-^e2COS02 
+m2l'o-C'e2(sin0oSin  (00+01+02)  +cos0ocos  (0o+0i+02) ) ) 
+0102  { J2+/n2L|2+m2LiJ^2Cos02) 
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APPENDIX  B:  MATIAB  CODE 


A.  PCONT 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  Main  Program  to  Track  a  Polynomial  Reference  Maneuver 
%  pcont.m 
%  calls:  ode2.m 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
clg 
clear 

%%%%%%%%%%%%%%%%% 

%  Define  States  % 

%%%%%%%%%%%%%%%%% 

%  x{l)=thdl 
%  X (2 ) =thd2 
%  x(3)=thd3 
%  x(4) =thl 
%  x(5)=th2 
%  x(6)=th3 


%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  Actual  System  Constants  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%% 


LO  = 

.427; 

% 

centerbody  r-  dius 

LI  = 

.530; 

% 

length  of  arm  1 

L2  = 

.533; 

% 

length  of  arm  2 

LcO 

=  .104; 

%  length  to  CM  of 

centerbody 

Lcl 

=  .403; 

%  length  to  CM  arm 

1 

Lc2 

=  .314; 

%  length  to  CM  arm 

2 

mO  = 

65.96; 

%  centerbody  mass 

ml  = 

2.34; 

%  arm  1  mass 

m2  = 

2.86; 

%  arm  2  mass 

10  = 

5.74; 

%  centerbody  inertia 

11  = 

.081; 

%  arm  1  inertia 

12  = 

.182; 

%  arm  to  inertia 

%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
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dP  dP  dO 


%  Actual  System  Parameters  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

aa(l)=  12  +  m2*Lc2''2; 

aa(2)=  m2*Ll*Lc2; 

aa{3)=  m2*L0*Lc2; 

aa(4)=  II  +  ml*Lcl"2  +  m2*Ll"2; 

aa{5)=  LO*(ml*Lcl  +  m2*Ll); 

aa{6)=  10  +  mO*LcO"2  +  L0"'2*(ml+m2); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Initial  Guess  of  System  Constants  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

error  =1;  %  maximum  error  for  constants 

LcOg  =  LcO+2*error*LcO* (rand- . 5) ; 

Lclg  =  Lcl+2*error*Lcl* (rand- . 5) ; 

Lc2g  =  Lc2+2*error*Lc2* (rand-.5) ; 

mOg  =  mO+2*error*mO* (rand- . 5) ; 
mlg  =  ml+2*error*ml* (rand- . 5) ; 
m2g  =  m2+2*error*m2* (rand- . 5) ; 

lOg  =  I0+2*error*10* (rand- . 5) ; 

Ilg  =  Il+2*error*Il* (rand- . 5) ; 

I2g  =  l2+2*error*l2* (rand- .5) ; 

%%%%%%%%%%%%%%%%%%%%%% 

%  Initial  Conditions  % 

%%%%%%%%%%%%%%%%%%%%%% 

aO  =  zeros (6,1) ; 

a0(l)  =  I2g  +  m2g*Lc2g''2; 

a0(2)  =  m2g*Ll*Lc2g; 

a0(3)  =  m2g*L0*Lc2g; 

a0(4)  =  Ilg  +  mlg*Lclg''2  +  m2g*Ll''2; 

a0(5)  =  LO* (mlg*Lclg  +  m2g*Ll) ; 

%  a0(6)  =  lOg  +  m0g*Lc0g''2  +  L0''2*  (mlg+m2g)  ; 
aO (6)  =  aa(6)  ; 

pO  =  100*eye ( 6) ; 

to  =  0;  ^ 

t final  =15  ; 
dt  =  .01; 

xO  =  [0.0;  0.0;  0.0;  0*pi/180;  -55*pi/180;  15*pi/180]; 
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tol  =  le-6; 
trace  =  0; 

%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Numerical  Integration  % 

%%%%%%%%%%%%%%%%%%%%%%%%% 

[t,x,  thdd,u,a,h,hd,Ref,  rxref,  ryref] 
eul ( 'peq' , tfinal,dt,x0,a0,p0) ; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Calculate  Momentum  Wheel  Speed  &  Tip  Position  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

Iw  =  0.0912;  %  kg-m^2 

AA  =  [aa'  ]  ; 

thddw  =  zeros (1, length(t) ) ; 

thdw  =  zeros (1, length(t) ) ; 

thdw(l)  =  104.7;  %  rad/sec  (=  lOOOrpm) 

rx  =  zeros (1, length(t) ) ; 

ry  =  zeros (1, length(t) ) ; 

r  X  {  1  ) 

L0*cos (X ( 1 , 4 ) ) +Ll*cos (sum(x (1, 4:5))) +L2*cos ( sum(x (1,4:6))) 
r  y  (  1  ) 

L0*sin (x (1, 4) ) +Ll*sin (sum(x(l, 4:5))) +L2*sin {sum(x(l , 4:6))) 

for  i=2 : length(t) ; 

thddw(i)  =-u{i,l)/Iw; 

thdw(i)  =  thddw(i) * (t (i) -t (i-l) ) +thdw(i-l) ; 

AA= [ AA  aa ' ] ; 

r  X  (  i  ) 

L0*cos (x (i , 4) ) +Ll*cos (sum(x(i, 4:5))) +L2*cos (sum(x (i , 4:6))) 
r  y  (  i  ) 

L0*sin {x( i , 4 ) ) +Ll*sin (sum(x (i , 4:5))) +L2*sin {sum(x (i , 4 : 6 ) ) ) 
end 

%%%%%%%%%%%%%%% 

%  Plot  Output  % 

%%%%%%%%%%%%%%% 

%  figplot 
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B.  PBQ 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  Equations  of  Motion  for  a  Polynomial  Reference  Maneuver  % 
%  peq.m  % 

%  called  by:  ode2.m 

%  calls:  ref.m,  mgm.m,  adap.m,  angmo.m 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

function  [xdot , thdd, U, A, P, H, Hd. Ref , rx,  ry]  =  peq(t,x,a,p) 

%%%%%%%%%%%%%%%%% 

%  Define  States  % 

%%%%%%%%%%%%%%%%% 

%  x(l)=thdO 
%  x(2)=thdl 
%  x{3)=thd2 
%  x(4)=th0 
%  x{5)=thl 
%  x(6) =th2 

%%%%%%%%%%%%% 

%  Constants  % 

%%%%%%%%%%%%% 

LO  =  .427;  %  centerbody  radius 

Ll  =  .530;  %  length  of  arm  1 

L2  =  .533;  %  length  of  arm  2 

L  =  [L0;L1;L2]; 

LcO  =  .104;  %  length  to  CM  of  centerbody 

Lcl  =  .403;  %  length  to  CM  arm  1 

Lc2  =  .314;  %  length  to  CM  arm  2 

Lc  =  [LcO ; Lcl ; Lc2 ] ; 

mO  =65.96;  %  centerbody  mass 

ml  =  2.34;  %  arm  1  mass 

m2  =  2.86;  %  arm  2  mass 

m  =  [m0;ml;m2]; 

10  =  5.74; 

11  =  .081; 

12  =  .182; 

I  =  [I0;I1;I2]; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Actual  System  Parameters  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

aa  (1)  =  12  +  m2*Lc2''2  ; 


%  centerbody  inertia 
%  arm  1  inertia 
%  arm  to  inertia 
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aa(2)=  in2*Ll*Lc2; 

aa(3)=  m2*L0*Lc2; 

aa(4)=  II  +  ml*Lcl^2  +  m2*Ll"2; 

aa{5)=  LO*(ml*Lcl  +  m2*Ll); 

aa(6)=  10  +  mO*LcO''2  +  L0"2*  (ml+m2 )  ; 


%%%%%%%%%%%%%% 

%  Controller  % 

%%%%%%%%%%%%%% 

thd (1,1)  =  X ( 1 ) ; 
thd(2, 1)  =  x(2) ; 
thdO.l)  =  x(3)  ; 

th(l,l)  =  x{4) ; 
th(2, 1)  =  x(5) ; 

th(3,l)  =  x(6); 

[MM,GM]  =  mgin{  th,  thd,  a)  ; 

Kp=100*eye (3 ) ; 

Kv=50*eye (3); 

[Uref , thr , thdr, thddr, rx, ry]  =  ref(t,L,a) 

Ref=lUref ;thr;thdr;thddr] ; 
du=MM* (-Kv* (thd-thdr) -Kp* (th-thr) ) ; 

U1 =MM* thddr +GM; 

U=Uref+du; 

%  U=Ul+du; 

%  U=Uref; 

%%%%%%%%%%%%% 

%  Plant  EOM  % 

%%%%%%%%%%%%% 

[MMa,GMa]  =  mgro  ( th, thd, aa) ; 

thdd  =  inv{MMa) * (U-GMa) ; 
xdot  =  [ thdd ; X ( 1 ) ; x { 2 ) ; x ( 3 ) ] ; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Adaptive  Parameter  Update  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

[A,P,Phi,K]  =  adap ( th, thd, thdd, a, U, p) ; 

%  test=MMa*thdd+GMa-Phi'*aa' 
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%%%%%%%%%%%%%%%%%%%%%%% 
%  Nonadaptive  Control  % 
%%%%%%%%%%%%%%%%%%%%%%% 


%  A  =  a; 

%  P  =  p; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Calculate  Angular  Momentum  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

[H,Hd]  =  angmo (m, I , L, Lc, th, thd, thdd) ; 
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C.  REF 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Function  to  produce  reference  parameters  % 

%  ref.m  % 

%  called  by:  peq.m  % 

%  calls:  mgm.m  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

function  [Uref , thr, thdr, thddr, rx, ry]  =  ref(t,L,a); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Initial  and  Final  Angles  and  Times  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

L0=L(1) ; 

L1=L(2) ; 

L2=L(3) ; 

thOri  =0;  %  always=0 

thlri  =  -55*pi/180; 
th2ri  =  15*pi/180; 

thOrf  =0;  %  always=0 

thlrf  =  40*pi/180; 
th2rf  =  15*pi/180; 

ths  =0;  %  constant 

to  =  0; 

tf  =  10; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Initial  And  Final  Vector  Positons  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

r3x0  =  L0*cos (ths) +Ll*cos (ths+thlri) +L2*cos (ths+thlri+th2ri) ; 
r3y0  =  L0*sin(ths)+Ll*sin(ths+thlri)+L2*sin(ths+thlri+th2ri) ; 
r3xf  =  L0*cos (ths) +Ll*cos (ths+thlrf ) +L2*cos ( ths+thlrf +th2rf )  ; 
r3yf  =  L0*sin(ths) +Ll*sin(ths+thlrf )+L2*sin(ths  +  thlrf+th2rf ) ; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Calculate  Reference  Maneuver  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

tau  =(t-t0)/  (tf-tO); 

f  =  (  10  *  tau"3  -  15  *  tau''4  +  6  *  taut's  )  ; 
fd  =  (  30  *  tau"2  -  60  *  tau''3  +  30  *  tau''4); 
f2d  =  (60*tau-180*tau"2  +  120*tau'"3)  ; 


78 


rx  =  r3x0  +  (  r3xf  -  r3x0  )  *  f; 
ry  =  r3y0  +  {  r3yf  -  r3y0  )  *  f; 


rxd  =  fd* {  r3xf  -  r3x0  )/(  tf  -  tO  ); 
ryd  =  fd* {  r3yf  -  r3y0  )/(  tf  -  tO  ); 

rxdd  =  f2d* (r3xf-r3x0) / ( (tf-tO) ^2) ; 
rydd  =  f2d*  (r3yf-r3y0)  /  (  (tf-tO) '^2)  ; 

if  {t>tf )  ; 

rx=r3xf ; 
ry=r3yf ; 
rxd=0 ; 
ryd=0 ; 
rxdd=0 ; 
rydd=0 ; 


end 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Determine  Inverse  Kinematics  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

Sx  =  L0*cos(ths); 

Sy  =  L0*sin ( ths ) ; 

SR  =  sgrt  (  (rx-Sx)  ^2+ (ry-Sy) ''2)  ; 

B1  =  atan2 (ry-Sy , rx-Sx) ; 

B2  =  acos  (  {L1^2  +  SR"2-L2''2)  /  (2*L1*SR)  )  ; 

B3  =  acos ( (L1"2+L2"2-SR"2) / {2*L1*L2) ) ; 

thlr  =  Bl-B2-ths; 

th2r  =  pi-B3; 

thr  =  [ 0 ; thlr ; th2r] ; 

%  thr  =  [thlr ; thlr ; th2r] ; 

=  -L2*sin ( ths+thlr+th2r) -Ll*sin ( ths+thlr) ; 

H(l,2)  =  -L2*sin ( ths+thlr+th2r) ; 

H(2,l)  =  L2*cos (ths+thlr+th2r) +Ll*cos (ths+thlr) ; 

H(2,2)  =  L2*cos (ths+thlr+th2r) ; 

[thdrl2]  =  inv (H) * [rxd; ryd] ; 
thdlr  =  thdrl2(l); 
thd2r  =  thdrl2(2); 
thdr  =  [ 0; thdlr ; thd2r ] ; 

%  thdr  =  [thdlr;thdlr;thd2r]; 

H  d  (  1  ,  1  ) 

-L2* ( thdlr+thd2r ) *cos ( ths+thlr+th2r ) -Ll*thdlr*cos (ths+thlr) 


79 


p 


Hd(l,2)  =  -L2*(thdlr+thd2r)*cos(ths+thlr+th2r); 

H  d  (  2  .  1  ) 

-L2* (thdlr+thd2r) *sin ( ths+thlr+th2r) -Ll*thdlr*sin ( ths+thlr) 
Hd(2,2)  =  -L2* (thdlr+thd2r) *sin (ths+thlr+th2r) ; 

[thddrl2]  =  inv (H) * { [rxdd; rydd] -Hd* [thdlr; thd2r] ) ; 
thddlr  =  thddrl2(l); 
thdd2r  =  thddrl2(2); 
thddr  =  tO;thddlr; thdd2r] ; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Calculate  Reference  Control  Torques  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

[MMr,GMr]  =  mgm(thr, thdr, a) ; 

Uref  =  MMr*thddr+GMr ; 


j 

I 
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D.  BUL 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%%%%%% 

%  Discrete  Euler  Integration 
% 

%  eul.m 
% 

%  called  byrpcont.m 
% 

%  calls:  peq.m 
% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%%%%%% 

function 

[ tout , xout , thddout , uout , aout , Hout , Hdout , Ref out , rxout , ryout ] = 

eul (FunFcn, tf , dt , xO , aO , pO ) 


%%%%%%%%%%%%%%%%%% 
%  Initialization  % 
%%%%%%%%%%%%%%%%%% 

t  =  0 ; 

X  =  xO ( : ) ; 


u  = 

zeros ( 3 , 

1)  ; 

a  = 

aO  (  :  )  ; 

thdd 

,  =  zeros 

(3,1)  ; 

P  = 

pO; 

H  = 

0; 

Hd  = 

0; 

Ref  = 

zeros  ( 12 

,1)  ; 

Ref  ( 

4:6)=[0; 

-55;15] *pi/180; 

LO  = 

.427; 

LI  = 

.530; 

L2  = 

.533; 

tout 

=  []  ; 

xout 

=  []; 

thddout  =  [  ]  ; 
uout  =  [  ]  ; 
aout  =  [ ] ; 

Hout  =  [ j ; 

Hdout  =  [ i ; 

Ref out  =  [ ] ; 
rxout  =  [  ]  ; 
ryout  =  [  ]  ; 

%%%%%%%%%%%%%%%%% 
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%  The  main  loop  % 
%%%%%%%%%%%%%%%%% 

for  i=l : (tf /dt ) +1 

[xd, thdd, u, A, P, H, Hd, Ref , rx, ry] 

tout  =  [tout;  t]; 
xout  =  [xout;  X.']; 
thddout  =  [ thddout ;  thdd . ' 1 ; 
uout  =  [uout;  u.']; 
aout  =  [ aout ;  a . ' ] ; 

Hout  =  [Hout;  H] ; 

Hdout  =  [Hdout;  Hd] ; 

Ref out  =  [Refout;  Ref.']; 
rxout  =  [rxout;rx]; 
ryout  =  [ryout;ry]; 

t  =  t  +  dt  ; 

X  =  X  +  xd*dt; 
a  =  A; 
p  =  P; 

end 


feval (FunFcn, t,x, a,p) ; 
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B.  MGM 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  Function  to  Calculate  'M'  matrix  and  'G'  vector  % 
%  mgm .  m  % 

%  called  by:  peq.m,  ref.m  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

function  =  mgm(th,thd,a); 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Define  Angles  &  Angular  Rates  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

thO  =  th ( 1 ) ; 
thl  =  th{2) ; 
th2  =  th ( 3 ) ; 
thdO  =  thd (1 ) ; 
thdl  =  thd ( 2 ) ; 
thd2  =  thd(3); 

%%%%%%%%%%%%%%%%%%%%%%%% 

%  Calculate  'M'  matrix  % 

%%%%%%%%%%%%%%%%%%%%%%%% 


MM(3, 3) 
MM{2, 3) 
MM(3 , 2) 
MM(1, 3) 
MM(3, 1) 
MM ( 2 , 2 ) 
MM(1, 2) 
MM(2, 1) 
MM(1, 1) 


a(l); 

MM(3,3)+a(2) »cos(th2) ; 

MM(2,3); 

MM(2 , 3 ) +a (3 ) *cos (thl+th2) ; 

MM(1,3); 

MM(2, 3) +a(2) *cos {th2)  +  a(4); 

MM(2 , 2 ) +a ( 3 ) *cos (thl+th2 )  +  a ( 5 ) *cos ( thl ) ; 

MM  (  1  ,  2  )  ; 

MM(2,2)+2*a(3) *cos { thl+th2 ) +2*a ( 5 ) *cos { thl ) +a ( 6) 


%%%%%%%%%%%%%%%%%%%%%%%% 
%  Calculate  'G'  vector  % 
%%%%%%%%%%%%%%%%%%%%%%%% 


ca2  =  thd2* (2*thd0+2*thdl+thd2) *sin(th2) ; 

GM(1,1)  =  -a(5)*(thdl^2+2*thd0*thdl)*sin(thl)-a{2)*ca2. . . 

-a  (3)  *  (2*thd0*  ( thdl  +  thd2  )  +  ( thdl+thd2  ) ''2 )  *sin  (thl  +  th2  )  ; 

G  M  (  2  .  1  ) 

a  ( 5  )  *thd0^2 *sin  ( thl )  -a  (2  )  *ca2+a  ( 3  )  *thd0''2 *sin  { thl  +  th2  )  ; 

G  M  (  3  ,  1  ) 

a(2)  *  (thdl+thd2)  "2*sin(th2)+a(3)  *thd0''2*sin(thl+th2)  ; 
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F.  AN6M0 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Subroutine  to  calculate  system  angular  nmomentum  % 
%  angmo.m 

%  called  by:  peq.m  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

function  [H, Hd] ^angmo (m, I , L, Lc, th, thd, thdd) ; 

%%%%%%%%%%%%%%%%%%%%%%%% 

%  Local  variable  names  % 

%%%%%%%%%%%%%%%%%%%%%%%% 

mO  =  m(l) ; 
ml  =  m  ( 2  )  ; 
m2  =  m  ( 3  )  ; 

10  =  1(1)  ; 

11  =  1(2)  ; 

12  =  1(3); 

LO  =  L(l)  ; 

LI  =  L(2) ; 

L2  =  L ( 3  )  ; 

LcO  =  Led)  ; 

Lcl  =  Lc(2) ; 

Lc2  =  Lc  ( 3 ) ; 

th0=th (1 ) ; 
thl=th(2) ; 
th2  =  th (3 )  ; 

thdO-thd (1); 
thdl=thd (2 ) ; 
thd2  =  thd(3)  ; 

thddO=thdd (1); 
thddl=thdd (2 ) ; 
thdd2=thdd(3 ) ; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Angular  Momentum  Equations  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

HO  =  thdO*  (lO+mO*LcO''2)  ; 

HI  =  thdO* (Il+ml* (L0"2+Lcl"2+2*L0*Lcl*cos (thl) ) ) . . . 

+  thdl*  (Il+ml*  (Lcl''2+L0*Lcl*cos  (thl)  )  )  ; 

H2  =  thdO* (l2+m2* (L0"2+Ll"2+Lc2"2+2*L0*Ll*Cos (thl) . . . 
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+2*Ll*Lc2*cos ( th2 ) +2*L0*Lc2*cos (thl+th2) ) ) . . . 
+thdl*  (l2+m2*  (Ll''2+Lc2''2+L0*Ll*cos  (thl)  .  .  . 

+2*Ll*Lc2*cos ( th2 ) +L0*Lc2*cos (thl+th2) ) ) . . . 
+thd2*  (l2+in2*  (Lc2''2+Ll*Lc2*cos  (th2  ) +L0*Lc2*cos  ( thl+th2  )  )  ) 

H=H0+H1+H2; 

%%%%%%%%%%%%%%%%%% 

%  Hdot  Equations  % 

%%%%%%%%%%%%%%%%%% 

HdO  =  thddO*  (I0+in0*Lc0''2)  ; 

Hdl  =  thddO*  (Il+ml*  (L0''2+Lcl"2+2*L0*Lcl*cos  (thl)  )  )  .  .  . 
+thddl*  (Il+ml*  (Lcl''2+L0*Lcl*cos  (thl)  )  )  .  .  . 
-thd0*thdl*2*inl*L0*Lcl*sin  (thl )  .  .  . 
-thdl''2*inl*L0*Lcl*sin  (thl)  ; 

Hd2  =  thddO*  (I2+in2*  (L0"2+Ll"2+Lc2''2+2*L0*Ll*cos  (thl)  .  .  . 

+2*Ll*Lc2*cos (th2) +2*L0*Lc2*cos (thl+th2) ) ) . . . 

+  thddl*  (I2+in2*  (Ll''2+Lc2^2+L0*Ll*cos  (thl)  .  .  . 

+2*Ll*Lc2*cos (th2)+L0*Lc2*cos (thl+th2) ) ) . . . 

+  thdd2*(l2+iTi2*(Lc2^2+Ll*Lc2*cos(th2)+L0*Lc2*cos(thl+th2)  )  )  .  . 
-thd0*thdl*2*m2* (L0*Ll*sin ( thl ) +L0*Lc2*sin ( thl+th2 ) ) . . 
-thd0*thd2*2*m2* (Ll*Lc2*sin ( th2) +L0*Lc2*sin ( thl+th2 ) ) . . 
-thdl*thd2*2*m2* (Ll*Lc2*sin(th2) +L0*Lc2*sin(thl+th2) ) . . 
-thdl''2*m2*  (L0*Ll*sin(thl)+L0*Lc2*bin(thl+th2)  )  .  .  . 
-thd2"2*m2* (Ll*Lc2*sin ( th2 ) +L0*Lc2*sin ( thl+th2 ) ) ; 

Hd=HdO+Hdl+Hd2; 
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O.  ADAP 

%%%%%%%%%%%%%%%%%%%%%%%%% 

%  Adaption  Law  % 

%  A=adap (th, thd, thdd, a)  % 

%  called  by:  peq.m  % 

%%%%%%%%%%%%%%%%%%%%%%%%% 

function  [A, P, Phi,K] =adap ( th, thd, thdd,a,U,p) ; 

%%%%%%%%%%%%%%%%%%%%%%%% 

%  Local  Variable  Names  % 

%%%%%%%%%%%%%%%%%%%%%%%% 

thO=th(l) ; 
thl=th(2) ; 
th2  =  th ( 3 )  ; 

thdO=thd (I); 
thdl=thd (2 ) ; 
thd2=thd(3) ; 

thddO=thdd ( 1 ) ; 
thddl=thdd (2 ) ; 
thdd2=thdd { 3 ) ; 

%%%%%%%%%%%%%% 

%  Phi  Matrix  % 

%%%%%%%%%%%%%% 

Phi (1,1)  =  thdd0+thddl+thdd2 ; 

Phi ( 1 , 2 )  =  Phi ( 1 , 1 ) ; 

Phi (1,3)  =  Phi ( 1 , 1 ) ; 

Phi (2,1)  =  (2*thdd0+2*thddl+thdd2 ) *cos (th2 ) . . . 

-thd2* (2*thd0+2*thdl+thd2) *sin(th2) ; 

Phi ( 2 , 2 )  =  Phi ( 2 , 1 )  ; 

Phi  (2, 3)  =  (thddO+thddl )  *cos  (th2 )  +  (thdl+thd2)''2*sin(th2); 
Phi (3,1)  =  (2*thdd0+thddl+thdd2) *cos (thl+th2 ) . . . 

-  (2*thd0*  (thdl+thd2 )  +  (thdl+thd2  ) ''2  )  *sin  (thl+th2  )  ; 
Phi (3, 2)  =  thddO*cos (thl+th2 )  +  thd0^2*sin(thl+th2); 

Phi (3, 3)  =  Phi(3,2); 

Phi (4,1)  =  thddO+thddl; 

Phi ( 4 , 2 )  =  Phi(4,l); 

Phi ( 4 , 3 )  =  0 ; 

Phi(5,l)  =  (  2  * t hdd 0  +  t hdd  1  )  * c o s  (  t h  1  ) 

(thdl''2+2*thd0*thdl)  *sin(thl)  ; 

Phi  (5, 2)  =  thddO*cos  ( thl )  +  thd0''2*sin(thl); 

Phi (5, 3)  =  0; 

Phi (6,1)  =  thddO; 

Phi (6,2)  =  0; 


86 


Phi (6, 3)  =  0; 


%%%%%%%%%%%%%%%%%%%%%% 

%  Adaption  Equations  % 
%%%%%%%%%%%%%%%%%%%%%% 

K  =  p*Phi*inv (eye (3 ) +Phi ' *p*Phi ) ; 

%  A=a  ; 

A  =  a+K* (U-Phi' *a) ; 

%  P  =  eye  ( 6 )  ; 

P  =  p-K*Phi ' *p+eye (6) ; 
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APPENDIX  C:  EXPERIMENTAL  CONTROL  BLOCK  DIAGRAMS 


This  appendix  includes  the  diagrams  of  the  system  build 
block  diagrams  made  to  control  the  SRS.  Both  is  the  parent 
superblock.  The  others  are  lower  level  superblocks. 
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Figure  45:  Controller  Block  Diagram 
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